package com.rockwell.robot2015.model.solvers;

import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.RobotAction;
import com.rockwell.robot2015.model.detectors.IDetector;
import com.rockwell.robot2015.model.sensors.IRobotSensors;

public class LoopMazeSolver implements IMazeSolver {
	int m_tries;
	int m_junctionOrder;
	boolean m_exiting;
	
	public LoopMazeSolver(boolean exiting) {
		m_exiting = exiting;
		m_tries = 0;
		m_junctionOrder = 0;
	}
	
	@Override
	public void reset(Robot robot) {
		m_tries = 0;
		m_junctionOrder = 0;
	}
	
	@Override
	public void onJunctionReached(Robot robot) {
		m_junctionOrder = 0;
	}

	@Override
	public void onFrontWallReached(Robot robot) {
	}

	@Override
	public void move(Robot robot) {
		IRobotSensors sensor = robot.getSensor();
		
		if (sensor.detectFrontWall() && sensor.detectLeftWall() && sensor.detectRightWall()) {
			robot.adjustAngle(Robot.ADJUST_ANGLE_THRESHOLD);
		}
		
		String log = "Left: " + sensor.detectLeftWall() + ", Right:"
				+ sensor.detectRightWall() + ", Front: " + sensor.detectFrontWall();
		robot.log(log, 4);

		if (m_tries > 0 && m_tries == m_junctionOrder) {
			// break the rule
			if (!sensor.detectRightWall()) {
				robot.log("Maze3, force to turn right", 4);
				
				robot.sendCommand(RobotAction.MoveWithoutSensor,
						RIGHT_TURN_CLEARANCE);
				robot.sleep(Robot.MOTION_SETTLING_TIME);

				robot.turnRight();
				robot.sleep(Robot.MOTION_SETTLING_TIME);
				
				return;
			}
			else if (!sensor.detectFrontWall()) {
				robot.sendCommand(RobotAction.MoveWithoutSensor,
						JUNCTION_CLEARANCE);
				robot.sleep(Robot.MOTION_SETTLING_TIME);

				return;
			}
		}
		
		if (!sensor.detectLeftWall()) {
			robot.sendCommand(RobotAction.MoveWithoutSensor,
					LEFT_TURN_CLEARANCE);
			robot.sleep(Robot.MOTION_SETTLING_TIME);

			robot.turnLeft();
			robot.sleep(Robot.MOTION_SETTLING_TIME);

			if (detectEntrance(robot)) {
				robot.log("Entrance detected again", 4);

				if (m_exiting) {
					return;
				}
				
				m_tries++;
				m_junctionOrder = 0;
				
				robot.turnRight();
				robot.sleep(Robot.MOTION_SETTLING_TIME);
				
				if (sensor.detectFrontWall() && !sensor.detectRightWall()) {
					robot.turnRight();
					robot.sleep(Robot.MOTION_SETTLING_TIME);
				}
			}
		} else if (!sensor.detectFrontWall()) {
			if (detectEntrance(robot)) {
				robot.log("Entrance detected", 4);

				if (m_exiting) {
					return;
				}
				
				m_tries++;
				m_junctionOrder = 0;
				
				if (sensor.detectFrontWall() && !sensor.detectRightWall()) {
					robot.turnRight();
					robot.sleep(Robot.MOTION_SETTLING_TIME);
				}
			}
			
		} else if (!sensor.detectRightWall()) {
			robot.sendCommand(RobotAction.MoveWithoutSensor,
					RIGHT_TURN_CLEARANCE);
			robot.sleep(Robot.MOTION_SETTLING_TIME);

			robot.turnRight();
			robot.sleep(Robot.MOTION_SETTLING_TIME);
		} else {
			robot.turnBack();
			robot.sleep(Robot.MOTION_SETTLING_TIME);
		}
	}

	private boolean detectEntrance(Robot robot) {
		IDetector detector = robot.getMazeEntranceGuideDetector();
		return detector.isPresent();
	}
}
